/*
 * dc_motor_current_controll.c
 *
 * Created:			05.08.2012 20:34:58
 * Author:			boecki91@gmail.com
 * Target:			atMEGA644P
 * URL:				$HeadURL: http://dc-motor-regelung.googlecode.com/svn/trunk/Software/dc_motor_current_controll.c $
 * Last changed by:	$Author: boecki91@gmail.com $
 * Changed date:    $Date: 2012-08-05 20:21:36 +0000 (Sun, 05 Aug 2012) $
 * Last committed:  $Revision: 12 $
 *
 * ID:				$Id: dc_motor_current_controll.c 12 2012-08-05 20:21:36Z boecki91@gmail.com $
 */
#include "global.h"
#include <avr/io.h>
#include <avr/interrupt.h>
#include "dc_motor_current_control.h"

double dif = 0;
double dif_old = 0;
double T_Ri = 0;
double k_Ri = 0;
double k_i = 0;
volatile double y_i = 0;
volatile double soll = 0;

void dc_motor_current_controll_init()
{
	//Calculate Parameters
	T_Ri = T_A;
	k_Ri = 2.0/3.0 * 1.0/(k_SR*k_A*k_MC)*T_A/(T_SR+T_MC);
	k_i = k_Ri * T_AR;
	soll = 500;
	//H-Bridge configuration
	DDRD = (1<<PD4) | (1<<PD5) | (1<<PD7);
	PORTD = (1<<PD7);
	
	//Clear 1A on Compare Match
	TCCR1A = (1<<COM1A1) | (1<<WGM11) | (1<<WGM10);
	//Phase correct PWM; Prescale:1
	TCCR1B = (1<<CS10);
	
	TCCR0A = (1<<WGM01);
	TCCR0B = (1<<CS01);
	OCR0A = PWM_VALUE_0;
	// Currentcontrol on:
	TIMSK0 = (1<<OCF0A);
}

void dc_motor_current_controll_cycle()
{

	dif = soll-adc_read(0);
	y_i = y_i +k_Ri*(dif-dif_old) +  k_i*dif_old;
	dif_old = dif;
	// Only 10bit allowed
	if(y_i >0x3FF)
	{
		OCR1A = 0x3FF;
	}
	else if(y_i <0)
	{
		OCR1A = 0;
	}
	else
	{
		OCR1A = y_i;
	}
}	
